//------------------------------------------------------------------------------
//  <copyright file="KukaLBR4HardwareTests.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#include "stdafx.h"
#include "CSMDGenerators.h"
#include "KukaLBR4HardwareTests.h"

/// <summary> Test that moves the LBR4 to a given joint position </summary>
/// <param name="pJointGoalPositions"> Target joint positions. </param>
/// <param name="pMessageHandler"> Controller client message hander for interfacing with with LBR </param>
void KukaLBR4HardwareTests::GoToPositionTest(double pJointGoalPositions[NUM_JOINTS], ControllerClientMessageHandler *pMessageHandler)
{
	double maxVelInRadPerSecond = 0.20;
	double cycleTimeInMs = 10;
	double movementTimeoutInMs = 120000;
	double feedbackUpdateRateInMs = 10;
	double positionErrorEpsilon = 0.01;

	MessageDescription *pRequest = new MessageDescription(sizeof(ControllerStateMachineDescription));
	MessageDescription *pResponse = new MessageDescription(sizeof(KukaFeedbackMessage));

	pRequest->IsResponse = 0;

	// set the csmd
	ControllerStateMachineDescription csmd = GenerateSimplePositionCsmd();
	pRequest->MessageType = MessageTypes::SetCsmdMessageType;
	pRequest->ParameterLength = sizeof(ControllerStateMachineDescription);

	memcpy_s(
		pRequest->pParameters,
		pRequest->ParameterBufferLength,
		&csmd,
		sizeof(ControllerStateMachineDescription));
	
	pMessageHandler->ProcessMessage(pRequest, pResponse);

	KUKALBR4NATIVEASSERT(pResponse->pParameters[0] == 1, "Failed to set csmd");

	// set the desired position and comparand for transition
	pRequest->MessageType = MessageTypes::SetWorkingRegistersMessageType;
	pRequest->ParameterLength = sizeof(KukaWorkingRegisterMessage);

	KukaWorkingRegisterMessage payload;
	for(int i = 0; i<NUM_JOINTS; i++)
	{
		payload.JointVec2Register[i] = pJointGoalPositions[i]; 
	}

	double durationInCycles = 5;
	double cycleIncrement = 1;
	double currentCycleInitialValue = 0;

	payload.Scalar1Register = 2;
	payload.Scalar2Register = maxVelInRadPerSecond;
	payload.Scalar3Register = cycleTimeInMs;
	payload.Scalar4Register = durationInCycles;
	payload.Scalar5Register = cycleIncrement;
	payload.Scalar6Register = currentCycleInitialValue;

	memcpy_s(
		pRequest->pParameters,
		pRequest->ParameterBufferLength,
		&payload,
		sizeof(KukaWorkingRegisterMessage));

	pMessageHandler->ProcessMessage(pRequest, pResponse);

	KUKALBR4NATIVEASSERT(pResponse->pParameters[0] == 1, "Failed to set machine registers");
	
	// start the csmd
	pRequest->MessageType = MessageTypes::StartCsmdMessageType;
	pRequest->IsResponse = 0;
	pRequest->ParameterLength = 0;

	pMessageHandler->ProcessMessage(pRequest, pResponse);

	if(pResponse->pParameters[0] != 1)
	{
		throw "Failed to start csmd";
	}

	ControllerStateMachineRegisters<double> *pFeedbackRegisters = new ControllerStateMachineRegisters<double>(
		KukaLBR4PlusTotalNumberOfRegisters,
		(unsigned int *)KukaLBR4PlusRegisterSizes);

	KukaFeedbackMessage feedback;
	bool positionReached = true;
	int waitLoopIterationCount = 0;

	// wait until position is reached
	while(true)
	{
		pRequest->MessageType = MessageTypes::GetFeedbackMessageType;
		pRequest->IsResponse = 0;
		pRequest->ParameterLength = 0;

		pMessageHandler->ProcessMessage(pRequest, pResponse);

		KUKALBR4NATIVEASSERT(pResponse->MessageType == MessageTypes::GetFeedbackMessageType, "Message type is Feedback");

		// check that response is valid (eg. that FRI has started up succesfully)
		if(pResponse->ParameterLength != 0)
		{
			memcpy_s(
				&feedback,
				sizeof(KukaFeedbackMessage),
				pResponse->pParameters,
				pResponse->ParameterBufferLength);

			positionReached = true;

			for (int i = 0; i < NUM_JOINTS; ++i)
			{
				if(fabs(pJointGoalPositions[i] - feedback.MeasuredJointPositionRegister[i]) > positionErrorEpsilon)
				{
					positionReached = false;
					break;
				}
			}

			if (positionReached)
			{
				break;
			}
		}

		if ((waitLoopIterationCount++ * feedbackUpdateRateInMs) > movementTimeoutInMs)
		{
			KUKALBR4NATIVEASSERT(false, "Movment timeout");
		}

		Sleep((DWORD)feedbackUpdateRateInMs);
	}

	positionReached = false;
}

/// <summary> Test that creates an instance of KukaLBR4Controller and moves the LBR4 to a given joint position </summary>
/// <param name="pJointGoalPositions"> Target joint positions. </param>
void KukaLBR4HardwareTests::GoToPositionTest(double pJointGoalPositions[NUM_JOINTS])
{

	KukaLBR4PlusController *pKukaController = new KukaLBR4PlusController();

	FRIServer *pFRIServer = new FRIServer();

	ControllerClientMessageHandler *pMessageHandler = pKukaController->StartKukaLBR4PlusController(pFRIServer);

	KukaLBR4HardwareTests::GoToPositionTest(pJointGoalPositions, pMessageHandler);

	delete pKukaController;
}

/// <summary> Test that moves the LBR4 to a given Cartesian position </summary>
/// <param name="pCartesianGoalPosition"> Target Cartesian positions. </param>
/// <param name="pMessageHandler"> Controller client message hander for interfacing with with LBR </param>
void KukaLBR4HardwareTests::GoToCartesianPositionTest(
	double pCartesianGoalPositions[CARTESIAN_FRAME_LENGTH],
	ControllerClientMessageHandler *pMessageHandler)
{
	double maxRotVelInRadPerSecond = 0.2;
	double maxLinVelInMPerSecond = 0.1;
	double cycleTimeInMs = 10;
	double feedbackUpdateRateInMs = 10;
	double movementTimeoutInMs = 120000;
	double positionErrorEpsilon = 0.01;

	MessageDescription *pRequest = new MessageDescription(sizeof(ControllerStateMachineDescription));
	MessageDescription *pResponse = new MessageDescription(sizeof(KukaFeedbackMessage));

	pRequest->IsResponse = 0;

	// set the csmd
	ControllerStateMachineDescription csmd = GenerateSimpleCartesianDeltaMoveCsmd();
	pRequest->MessageType = MessageTypes::SetCsmdMessageType;
	pRequest->ParameterLength = sizeof(ControllerStateMachineDescription);

	memcpy_s(
		pRequest->pParameters,
		pRequest->ParameterBufferLength,
		&csmd,
		sizeof(ControllerStateMachineDescription));
	
	pMessageHandler->ProcessMessage(pRequest, pResponse);

	KUKALBR4NATIVEASSERT(pResponse->pParameters[0] == 1, "Failed to set csmd");

	// set the desired position and comparand for transition
	pRequest->MessageType = MessageTypes::SetWorkingRegistersMessageType;
	pRequest->ParameterLength = sizeof(KukaWorkingRegisterMessage);

	KukaWorkingRegisterMessage payload;

	for (int i = 0; i< CARTESIAN_FRAME_LENGTH; ++i)
	{
		payload.Frame1Register[i] = pCartesianGoalPositions[i];
	}

	payload.Scalar1Register = 2;
	payload.Scalar2Register = maxLinVelInMPerSecond;
	payload.Scalar3Register = maxRotVelInRadPerSecond;
	payload.Scalar4Register = cycleTimeInMs;

	memcpy_s(
		pRequest->pParameters,
		pRequest->ParameterBufferLength,
		&payload,
		sizeof(KukaWorkingRegisterMessage));

	pMessageHandler->ProcessMessage(pRequest, pResponse);

	KUKALBR4NATIVEASSERT(pResponse->pParameters[0] == 1, "Failed to set machine registers");
	
	// start the csmd
	pRequest->MessageType = MessageTypes::StartCsmdMessageType;
	pRequest->IsResponse = 0;
	pRequest->ParameterLength = 0;

	pMessageHandler->ProcessMessage(pRequest, pResponse);

	if(pResponse->pParameters[0] != 1)
	{
		throw "Failed to start csmd";
	}

	ControllerStateMachineRegisters<double> *pFeedbackRegisters = new ControllerStateMachineRegisters<double>(
		KukaLBR4PlusTotalNumberOfRegisters,
		(unsigned int *)KukaLBR4PlusRegisterSizes);

	KukaFeedbackMessage feedback;
	bool positionReached = true;
	int waitLoopIterationCount = 0;

	// wait until position is reached
	while(true)
	{
		pRequest->MessageType = MessageTypes::GetFeedbackMessageType;
		pRequest->IsResponse = 0;
		pRequest->ParameterLength = 0;

		pMessageHandler->ProcessMessage(pRequest, pResponse);

		if(pResponse->ParameterLength != 0)
		{
			KUKALBR4NATIVEASSERT(pResponse->MessageType == MessageTypes::GetFeedbackMessageType, "Message type is Feedback");

			memcpy_s(
				&feedback,
				sizeof(KukaFeedbackMessage),
				pResponse->pParameters,
				pResponse->ParameterBufferLength);

			positionReached = true;

			for (int i = 0; i < CARTESIAN_FRAME_LENGTH; ++i)
			{
				if(fabs(pCartesianGoalPositions[i] - feedback.MeasuredCartesianPositionRegister[i]) > positionErrorEpsilon)
				{
					positionReached = false;
					break;
				}
			}

			if (positionReached)
			{
				break;
			}
		}

		if ((waitLoopIterationCount++ * feedbackUpdateRateInMs) > movementTimeoutInMs)
		{
			KUKALBR4NATIVEASSERT(false, "Movment timeout");
		}

		Sleep((DWORD)feedbackUpdateRateInMs);
	}
}

/// <summary> Test that moves the LBR4 to a given Cartesian position </summary>
/// <param name="pCartesianGoalPosition"> Target Cartesian positions. </param>
void KukaLBR4HardwareTests::GoToCartesianPositionTest(
	double pCartesianGoalPositions[CARTESIAN_FRAME_LENGTH])
{
	KukaLBR4PlusController *pKukaController = new KukaLBR4PlusController();

	FRIServer *pFRIServer = new FRIServer();

	ControllerClientMessageHandler *pMessageHandler = pKukaController->StartKukaLBR4PlusController(pFRIServer);

	KukaLBR4HardwareTests::GoToCartesianPositionTest(
		pCartesianGoalPositions,
		pMessageHandler);

	delete pKukaController;
}

/// <summary> Test that information is received over the FRI connection </summary>
void KukaLBR4HardwareTests::TestFRIConnection()
{
	FRIServer *server = new FRIServer();

	ControllerStateMachineRegisters<double> *pFeedbackRegisters =
		new ControllerStateMachineRegisters<double>(
		KukaLBR4PlusFeedbackRegisters::NumberOfFeedbackRegisters,
		(unsigned int *)&(KukaLBR4PlusRegisterSizes[KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters]));

	int result = server->GetFRIRequest(pFeedbackRegisters->GetRegisters());

	KUKALBR4NATIVEASSERT(0 == result, "GetFRIRequest Succedes");

	server->CloseFRI();

	delete pFeedbackRegisters;
	delete server;
}

/// <summary>
/// Send a rapid sequence messages to the ControllerClientMessageHandler corresponding to a simple repetitive joint motion
/// </summary>
/// <param name="pMsgHandler"> The ControllerclientMessagehandler to interact with. </param>
void KukaLBR4HardwareTests::ExecuteJointPositionFloodTest(ControllerClientMessageHandler *pMsgHandler)
{
	unsigned int messageParameterBufferLength = max(sizeof(ControllerStateMachineDescription), sizeof(KukaLBR4PlusWorkingRegisters));
	unsigned int resonseParameterBufferLength = sizeof(KukaFeedbackMessage);
	MessageDescription *pMessage = new MessageDescription(messageParameterBufferLength);
	MessageDescription *pResponse = new MessageDescription(resonseParameterBufferLength);

	double jointSpeed = 0.1;
	double cycleTime = 10;
	double durationInCycles = 5;
	double cycleIncrement = 1;
	double currentCycleInitialValue = 0;
	int iterationTimeInMs = 7000;
	int currentTarget = 0;

	for(int iterations = 0; iterations < 10; ++iterations)
	{
		ULONGLONG iterationStartTimeMs = GetTickCount64();
		while (GetTickCount64() - iterationStartTimeMs < iterationTimeInMs)
		{
			// send the csmd
			pMessage->MessageType = MessageTypes::SetCsmdMessageType;
			pMessage->IsResponse = 0;
			pMessage->ParameterLength = sizeof(ControllerStateMachineDescription);

			ControllerStateMachineDescription csmd = GenerateSimplePositionCsmd();
			memcpy_s(
				pMessage->pParameters,
				pMessage->ParameterBufferLength,
				&csmd,
				sizeof(ControllerStateMachineDescription));

			pMsgHandler->ProcessMessage(pMessage, pResponse);

			// set the target position register
			pMessage->MessageType = MessageTypes::SetWorkingRegistersMessageType;
			pMessage->IsResponse = 0;
			pMessage->ParameterLength = sizeof(KukaWorkingRegisterMessage);

			// for hand built state machine used, the use of the relevant working registers is:
			// JointVec2Register:  joint position goal
	                // Scalar1Register:  FRI command mode number
	                // Scalar2Register:  Maximum joint speed
	                // Scalar3Register:  Cycle time.
	                // Scalar4Register:  Duration in cycles.
	                // Scalar5Register:  Cycle increment value (typically 1).
	                // Scalar6Register:  Current cycle.
			KukaWorkingRegisterMessage payload;

			for (int i = 0; i<NUM_JOINTS; ++i)
			{
				payload.JointDampingCommandRegister[i] = 0;
				if(currentTarget == 0)
				{
					payload.JointVec2Register[i] = pCsmdFloodTarget1[i];
				}
				else
				{
					payload.JointVec2Register[i] = pCsmdFloodTarget2[i];
				}
			}

			// Used to determine when FRI is in command mode
			payload.Scalar1Register = 2;
			payload.Scalar2Register  = jointSpeed;
			payload.Scalar3Register = cycleTime;
			payload.Scalar4Register = durationInCycles;
			payload.Scalar5Register = cycleIncrement;
			payload.Scalar6Register = currentCycleInitialValue;

			memcpy_s(
				pMessage->pParameters,
				pMessage->ParameterBufferLength,
				&payload,
				sizeof(KukaWorkingRegisterMessage));

			pMsgHandler->ProcessMessage(pMessage, pResponse);

			// start the controller
			pMessage->MessageType = MessageTypes::StartCsmdMessageType;
			pMessage->IsResponse = 0;
			pMessage->ParameterLength = 0;

			pMsgHandler->ProcessMessage(pMessage, pResponse);
		}
		currentTarget = (currentTarget + 1) % 2;
	}

	// shut down the controller
	pMessage->MessageType = MessageTypes::ConnectionClosedMessageType;
	pMessage->IsResponse = 0;
	pMessage->ParameterLength = 0;
	pMsgHandler->ProcessMessage(pMessage, pResponse);

	delete pMessage;
	delete pResponse;
}

/// <summary> Move the arm to the "candle" position </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void MoveToCandlePositionTest(char *resultString, int bufferSize)
{
	double candlePosition[7] = {0, 0, 0, 0, 0, 0, 0};

	try
	{
		KukaLBR4HardwareTests::GoToPositionTest(candlePosition);
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);

}

/// <summary> Test move of the LBR using cartesian impedance control. </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void MoveTestCartesian(char *resultString, int bufferSize)
{
	double testPos1[12] = 
	{
		0.22690424323081970,
		0.97315621376037598,
		-0.038489356637001038,
		-0.093268766999244690,
		0.059803504496812820,
		0.025523381307721138,
		0.99788379669189453,
		0.82020062208175659,
		0.97207921743392944,
		-0.22872586548328400,
		-0.052406787872314453,
		0.13197985291481018
	};

	try
	{
		KukaLBR4HardwareTests::GoToCartesianPositionTest(
			testPos1);
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test that data can be received over the FRI connection </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestFRIConnection(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4HardwareTests::TestFRIConnection();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}


/// <summary> Test flooding controller running HW with new controller state machine requests.  Effect should be first and last joint rotating back and forth (in candle position) </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void CsmdFloodTest(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4PlusController *pKukaController = new KukaLBR4PlusController();

		FRIServer *pFRIServer = new FRIServer();

		ControllerClientMessageHandler *pMessageHandler = pKukaController->StartKukaLBR4PlusController(pFRIServer);

		KukaLBR4HardwareTests::ExecuteJointPositionFloodTest(pMessageHandler);
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}
